#include "../../Helix.h"
#include "../../HObject.h"
#include "../../Log.h"
#include "../../Math/HelixMath.h"
#include "../HelixMesh.h"
#include <ode/ode.h>
#include "Physics.h"
#include "ODEPhysics.h"
#include "PhysObjRepresentation.h"
#include "PhysObject.h"
#include "ODEPhysObject.h"



namespace helix
{
	ODEPhysObject::ODEPhysObject(ODEPhysics *physics,PhysRepresentation *representation,bool isStatic)
	{
		this->physics = physics;
		this->staticGeom = isStatic;
		
		initPhysicsRep(representation);
		dGeomSetData(geometry,this);
	}

	ODEPhysObject::~ODEPhysObject(){
		physics = NULL;
		dBodyDestroy(body);
	}

	void ODEPhysObject::initPhysicsRep(PhysRepresentation *representation)
	{
		//If this is not static, then it means that it has a mass and a body
		if(!staticGeom)
		{
			this->mass = representation->getMass();
			this->body = dBodyCreate(physics->getWorldId());
		}

		SphereRepresentation rep(representation->getMass(),0.001);
		switch(representation->getType())
		{
			case PhysRepresentation::SPHERE:
				initPhysicsRepSphere((SphereRepresentation*)representation);
				break;
			case PhysRepresentation::BOX:
				initPhysicsRepBox((BoxRepresentation*)representation);
				break;
			case PhysRepresentation::PLANE:
				initPhysicsRepPlane((PlaneRepresentation*)representation);
				break;
			default:
				initPhysicsRepSphere(&rep);
				break;
		}
	}

	void ODEPhysObject::initPhysicsRepSphere(SphereRepresentation *representation)
	{
		geometry = dCreateSphere (physics->getSpaceId(),representation->getRadius());
		
		//For ODE, you do not set a mass or body on static objects
		if(!staticGeom)
		{
			dMass massDistrobution;	
			dMassSetSphereTotal(&massDistrobution,representation->getMass(),representation->getRadius());
			dBodySetMass (body,&massDistrobution);
			dGeomSetBody (geometry,body);
		}
	}

	void ODEPhysObject::initPhysicsRepBox(BoxRepresentation *representation)
	{
		geometry = dCreateBox (physics->getSpaceId(),
			representation->getXLength(),
			representation->getYLength(),
			representation->getZLength());

		//For ODE, you do not set a mass on static objects
		if(!staticGeom)
		{
			dMass massDistrobution;	
			dMassSetBoxTotal(&massDistrobution,
				representation->getMass(),
				representation->getXLength(),
				representation->getYLength(),
				representation->getZLength());
			dBodySetMass (body,&massDistrobution);
			dGeomSetBody (geometry,body);
		}	
	}

	void ODEPhysObject::initPhysicsRepPlane(PlaneRepresentation *representation)
	{
		//Planes can only be static
		staticGeom = true;
		geometry = dCreatePlane (physics->getSpaceId(),
			representation->getA(),
			representation->getB(),
			representation->getC(),
			representation->getD());

	}

	void ODEPhysObject::setPosition(Vector pos)
	{
		if(staticGeom)
			dGeomSetPosition(geometry,pos.x(),pos.y(),pos.z());
		else
			dBodySetPosition(body,pos.x(),pos.y(),pos.z());
	}

	void ODEPhysObject::setMass(float mass)
	{
		Log().Error("Setting the Mass after creation is not supported in the ODE implementation at this time.");
	}

	void ODEPhysObject::setVelocity(Vector vel)
	{
		if(staticGeom)
			dBodySetLinearVel(body,vel.x(),vel.y(),vel.z());
	}

	Vector ODEPhysObject::getPosition()
	{
		Vector position;
		const dReal *pos; 
		if(staticGeom)
			pos = dGeomGetPosition(geometry);
		else
			pos = dBodyGetPosition(body);

		return Vector(pos[0],pos[1],pos[2]);
	}

	Vector ODEPhysObject::getVelocity()
	{
		if(staticGeom)
			return Vector();

		const dReal *vel = dBodyGetLinearVel(body);
		return Vector(vel[0],vel[1],vel[2]);
	}

	float ODEPhysObject::getMass()
	{
		if(staticGeom)
			return -1.0f;
		
		return this->mass;
	}

	bool ODEPhysObject::isStatic()
	{
		return staticGeom;
	}

	Matrix4x4 ODEPhysObject::getTransformationMatrix()
	{
		const dReal *pos;
		const dReal *rot;
		if(staticGeom)
		{
			pos = dGeomGetPosition(geometry);
			rot = dGeomGetRotation(geometry);
		}
		else
		{
			pos = dBodyGetPosition(body);
			rot = dBodyGetRotation(body);
		}

		//The matrix returned by ODE is a row-major 4x3 matrix
		//What this means for us is that we don't care about the
		//last value of each row, and we need to add a 4th row
		//of 0,0,0,1 to make this a standard 4x4 rotation matrix
		//float rotMat[4][4] = {{rot[0],rot[1],rot[2],0.0},
		//					  {rot[4],rot[5],rot[6],0.0},
		//					  {rot[8],rot[9],rot[10],0.0},
		//					  {0.0,0.0,0.0,1.0}};
		//Matrix4x4 position(pos[0],pos[1],pos[2]);
		//Matrix4x4 rotation(rotMat);
		//return rotation*position;

		float rotMat[4][4] = {{rot[0],rot[1],rot[2],pos[0]},
							  {rot[4],rot[5],rot[6],pos[1]},
							  {rot[8],rot[9],rot[10],pos[2]},
							  {0.0,0.0,0.0,1.0}};
		return Matrix4x4(rotMat);
	}
}